


#include "Matrix3.h"

#include <cmath>
#include <stdio.h>
#include <float.h>


	bool Matrix3::invert()
	{
		Vec4 col0 = getColumn(0);
		Vec4 col1 = getColumn(1);
		Vec4 col2 = getColumn(2);
		
		Vec4 r0 = col1 % col2; 
		Vec4 r1 = col2 % col0; 
		Vec4 r2 = col0 % col1; 
		float det = col0 * r0;
		
		const float epsilon = FLT_MIN;
		if (fabs(det) > epsilon*epsilon*epsilon)
		{
			float detinv = 1.f/det;
			setColumn(0, detinv*r0);
			setColumn(1, detinv*r1);
			setColumn(2, detinv*r2);
			transpose();
			return true;
		}
		return false;
	}
	
	void Matrix3::invertSymmetricPositiveDefinite()
	{
		Vec4 col0 = getColumn(0);
		Vec4 col1 = getColumn(1);
		Vec4 col2 = getColumn(2);
		
		Vec4 r0 = col1 % col2; 
		Vec4 r1 = col2 % col0; 
		Vec4 r2 = col0 % col1; 
		float det = fmaxf(FLT_MIN, col0 * r0);
		
		float detinv = 1.f/det;
		setColumn(0, detinv*r0);
		setColumn(1, detinv*r1);
		setColumn(2, detinv*r2);
		transpose();
	}
	
	
	float Matrix3::trace()
	{
		return m_matrix[0] + m_matrix[4] + m_matrix[8];
	}
	
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	/// Diagonalization of symmetric matrix - based on sec 8.4, Jacobi Methods, of Golub & Van Loan
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	
	struct SchurMatrix
	{
		float c, s;
		int p, q;
	};
	
	static inline void _constructSchurMatrix(const Matrix3& M, int p, int q, SchurMatrix& S)
	{
		S.p = p;
		S.q = q;
		float Mpq = M(p,q);
		if ( Mpq != 0.0f )
		{
			float tau = ( M(q,q) - M(p,p) ) / (2.0f * Mpq);
			float t = sqrtf(1.0f + tau*tau);
			if (tau>=0.0f) t = 1.0f / (tau + t);
			else t = 1.0f / (tau - t);
			S.c = 1.0f / sqrtf(1.0f + t*t);
			S.s = t * S.c;
		}
		else
		{
			S.c = 1.0f;
			S.s = 0.0f;
		}
	}
	
	static inline float _frobeniusNormSqr(const Matrix3& M)
	{
		Vec4 c0 = M.getColumn(0);
		Vec4 c1 = M.getColumn(1);
		Vec4 c2 = M.getColumn(2);
		return c0*c0 + c1*c1 + c2*c2;
	}
	
	static inline float _offDiagNormSqr(const Matrix3& M)
	{
		float m01 = M(0,1);
		float m02 = M(0,2);
		float m12 = M(1,2);
		return 2.f*(m01*m01 + m02*m02 + m12*m12);
	}
	
	static inline void _findLargestOffDiagEntry(const Matrix3& M, int& p, int& q)
	{
		p = 0; q = 1;
		float maxent = fabs(M(0,1));
		float mag = fabs(M(0,2));
		if ( mag > maxent ) { p = 0; q = 2; maxent = mag; }
		mag = fabs(M(1,2));
		if ( mag > maxent ) { p = 1; q = 2; }
	}
	
	static inline void _constructJacobiRotation(Matrix3& J, const SchurMatrix& S)
	{
		J.setIdentity();
		J(S.p, S.p) = S.c;
		J(S.p, S.q) = S.s;
		J(S.q, S.p) = -S.s;
		J(S.q, S.q) = S.c;
	}
	
	inline bool Matrix3::diagonalizeSymmetric(Matrix3& eigenVec, Vec4& eigenVal, int maxIter, float epsilon) const
	{
		// TO DO: support warm starting, maybe separate function
		
		Matrix3 M(*this);
		eigenVec.setIdentity();
		
		float epsSqr = epsilon*epsilon * _frobeniusNormSqr(*this);
		int nIter = 0;
		float normSqr = 0.0f;
		
		for (;;)
		{
			normSqr = _offDiagNormSqr(M);
			if ( normSqr<epsSqr || nIter>=maxIter ) break;
			
			int p = 0, q = 0;
			_findLargestOffDiagEntry(M, p, q);
			
			SchurMatrix S;
			_constructSchurMatrix(M, p, q, S);
			
			Matrix3 J;
			_constructJacobiRotation(J, S);
			
			// M <- J^T M J
			M = J.getTranspose() * (M * J);
			
			// V <- V J
			eigenVec *= J;
			
			nIter++;
		}
		
		eigenVal[0] = M(0,0);
		eigenVal[1] = M(1,1);
		eigenVal[2] = M(2,2);
		return normSqr < epsSqr; 
	}
	

	void Matrix3::polarDecompose(Matrix3& symmetricOut, Matrix3& rotationOut, int maxIter, float epsilon) const
	{
		const Matrix3& me = (*this);
		Matrix3 Tr = getTranspose();
		Matrix3 M = getTranspose() * me;
		
		Matrix3 U;
		Vec4 D;
		M.diagonalizeSymmetric(U, D, maxIter, epsilon);
		D.fabs4();
		
		Matrix3 TrU = U.getTranspose();
		
		// We can sometimes get an eigenvalue of zero, due to exactly planar geometry or just numerical roundoff.
		// Adding an epsilon here to the eigenvalues (after abs and sqrt) keeps the polar decomposition robust in these cases.
		Matrix3 SqD; SqD.setZero();
		SqD(0,0) = sqrtf(D[0]) + FLT_EPSILON; // should be a setDiag().
		SqD(1,1) = sqrtf(D[1]) + FLT_EPSILON; 
		SqD(2,2) = sqrtf(D[2]) + FLT_EPSILON;
		
		symmetricOut = U * (SqD * TrU);
		
		Matrix3 InvS = symmetricOut; 
		InvS.invertSymmetricPositiveDefinite();
		
		rotationOut = (*this) * InvS;
	}
	




